Q-METHOD EXTENDED KALMAN FILTER 


Renato Zanetti, Thomas Ainscoughf John Christian* and Pol D. Spanos 


A new algorithm is proposed that smoothly integrates non-linear estimation of 
the attitude quaternion using Davenport’s q-method and estimation of non-attitude 
states through an extended Kalman filter. The new method is compared to a similar 
existing algorithm showing its similarities and differences. The validity of the 
proposed approach is confirmed through numerical simulations. 


INTRODUCTION 

The well-known Wahba Problem [1] is a non-linear, weighted least-squares problem that seeks to 
obtain the optimal attitude matrix from a set of at least two independent vector measurements. The 
most common technique used to solved the Wahba problem is the so-called q-method, developed by 
Davenport and documented in [2], The q-method rearranges the Wahba performance index into a 
quadratic performance index of the attitude quaternion, which is constrained to have unit norm. The 
extremals of this performance index arc the eigenvalues of the Davenport matrix, and the optimal 
quaternion is the unit eigenvector corresponding to the largest eigenvalue. 

A variety of numerical approaches exist for calculating the maximum eigenvalue and correspond- 
ing eigenvector of the Davenport matrix. For example, the QUEST algorithm [3] calculates the 
eigenvalue using a Newton-Raphson method and the eigenvector by factoring the quaternion as a 
vector of Rodrigues parameters. To avoid the singularity of the Rodrigues parameters the method 
of successive rotations is also introduced in [3], Alternatively, ESOQ [4] avoids the singularity by 
computing the quaternion as a vector cross product in four dimensions. In a follow-on algorithm, 
ESOQ-2 [5], the Euler axis is computed as the null space of a 3 x 3 matrix that is derived from the 
Davenport matrix. 

QUEST, ESOQ, and ESOQ-2 are numerical implementations of Davenport’s q-method. Other 
numerical techniques exist that compute the attitude matrix directly rather than the quaternion. One 
such technique from Markley is based on the Singular - Value Decomposition (SVD) [6], Here, it 
should be noted that the original Wahba problem objective function is fundamentally just a special 
case of the Orthogonal Procrustes Problem , which has received a considerable amount of study 
since the 1950s [7], 
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One of the reasons that the Wahba problem has received so much attention is that it provides 
a globally optimal solution and it does not make any linearization or small angle approximations. 
Conversely, the workhorse of aerospace estimation, the extended Kalman filter (EKF) [8] relies on 
linearization to obtain an estimate. The solution to the Wahba problem provides single point attitude 
estimates and requires all the measurements to be synchronized. The EKF and its attitude-specific 
extensions (most notably additive EKF [9] and multiplicative EKF [10]) in contrast are recursive 
estimators. 

With this in mind, a number of algorithms have been developed to reformulate Davenport’s solu- 
tion into a recursive algorithm. Two of the first such methods are Filter QUEST [1 1] and REQUEST 
[12], which arc both sub-optimal filters capable of estimating attitude (but not other states, such as 
biases). Later, Filter QUEST and REQUEST were shown to be two different formulations of math- 
ematically equivalent filters [13]. Subsequently, the Optimal-REQUEST filter [14] addressed the 
sub-optimality of these filters, but was still not capable of estimating non-attitude states. 

Markley [15] shows how to estimate not only attitude, but also other parameters such sensor 
biases from vector observations. Extended-QUEST also estimates attitude and non-attitude states 

[16] . This work introduces a novel EKF-based estimation algorithm that integrates the q-method 
to process attitude vector measurements. The existing algorithm that most closely resembles the 
present work is the Sequential Optimal Attitude Recursion (SOAR) filter by Christian and Lightsey 

[17] . The key difference is that SOAR uses the information formulation of the Kalman filter for the 
measurement update while the proposed method is a covariance formulation. This difference will 
usually require smaller matrix inversions when the size of the state vector is large. Another differ- 
ence between the two methods is how the initial condition is introduced into the Wahba problem. 
This paper uses quaternion averaging [18], while SOAR uses the information matrix approach by 
Shuster [19]. In spite of these differences, the proposed q-method EKF (qEKF) and the SOAR filter 
are shown to be equivalent to second-order in the attitude update and first-order in the non-attitude 
state update. Hence qEKF and SOAR can be considered the covariance and information approaches 
to the solution of the same problem. 

The proposed algorithm smoothly integrates the q-method into the EKF framework. S im ilar to 
the SOAR filter and Extended QUEST, the proposed algorithm processes the vector measurements 
first and the remaining quantities last. However, unlike Extended QUEST, both the SOAR filter and 
qEKF do not necessitate numerical iterations. Shuster [20] suggests that numerical solutions to the 
q-method such as QUEST could be used as a pre -processor for the EKF. The proposed algorithm 
takes this concept one step further by integrating the q-method into the EKF. 

THE WAHBA PROBLEM 

Re-written in terms of the inertial-to-body quaternion q, the Wahba problem consists of minimiz- 
ing the performance index 


1 2 

mjn J (q) = - V a* ||y* - T(q)n ?: || , 

i = 1 

where are vector observations and are their representation in the reference frame. 
In the absence of noise, the perfect measurement is simply given by 

Yi = T(q) n* 


( 1 ) 


( 2 ) 
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In the presence of uncertainty, this becomes (omitting the dependency on q) 


(3a) 

(3b) 


y* = Tn, + Sy t 
fi; = n; + Srii 


since ||y,;|| = | y | = 1 and ||n,;|| = jn, | = 1, the following is also true to first order 

yf fyi ~ yj Sy i ~ 0 n f Siii ^ n J 8n % ^ 0 (4) 

This leads directly to the QUEST measurement model [3] for a unit vector observation, 

Rnn = cTn ( J 3x3 - nn T ) (5a) 

R yy = Cy (1-3x3 “ yy T ) (5b) 

Substituting this result into Eq. (1) (and assuming that Sy l and bn, are uncoiTelated) shows that for 
q to be a maximum likelihood estimate of the attitude (to first order) the weights cq should be the 
following 


(H « 1/ (cr„ + CJy) (6) 

Returning to Eq. (1), the goal is now to reformulate the problem in terms of the attitude quater- 
nion. Begin by recalling that the coordinate transformation matrix written as a function of the 
quaternion is given by 


T = T (q) = I 3x3 - 2 q 4 [q„x] + 2 [q„x] 2 (7a) 

= (d - qjq?;) 1-3x3 - 2 q 4 [q«x] + 2q,„qJ (7b) 

The minimization of the Wahba performance index in Eq. (1) is now equivalent to the maximization 
of 


max J* (q) = trace [T (q) B 1 ] = q T Kq, (8) 

where the 4x4 Davenport matrix K is obtained as 

n 

z = Y2 a i (y* x 

i = 1 

a = trace (B) 


the optimal quaternion is the unit eigenvector of K associated with the maximum eigenvalue. 

In this work K is slightly modified to perform covariance analysis. The performance index is 
equivalently rewritten as 


B = 

2—1 

S = B + B t 

-wr rs-fjl 3x3 Z 


J* (q) = (T + q 


M z 
z T 0 


q, 


(9) 
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hence the optimal quaternion is the unit eigenvector of the matrix in Eq. (9) corresponding to its 
maximum eigenvalue and 

n 

M = ^2 a i ([y;x] [rq x ] + [n*x] [y*x]) = S - 2 ctI 3x3 . (10) 

2 = 1 


Recall that the perfect measurements y, are defined as y, = T n, where T is the true attitude 
matrix and n, are error-free reference vectors. By using y, and n, in place of y, and n, in the q- 
method the true quaternion q is obtained. Matrix B true is computed with the perfect values y, and 
n,. When the vectors y, and T(q)n, are used as the inputs in the q-method the identity quaternion 
is obtained; with this approach we arc estimating the deviation from the true body frame which is 
denoted as <5q*, the superscript indicates the quaternion conjugate. Using y, and T(q)n* to 
calculate matrix B we obtain T (q) B trU e~ hence the performance index is given by 

^W) = trace [T (5c?) T (q) Bj rue ] , (11) 

Notice that the combination of having perfect measurements and replacing n, with T(q)rq results 
in z = 0, which makes the performance index 




a + ciq* T 


B-true 

0 T 



( 12 ) 


where 

n 

H true = ^ ^ a i ([y* x ] [(Trij)x] + [(Tnj)x] [y,x]) 
i = 1 

n n 

= (y* y * T - Isx3 ) = 2^ai[yiX] 2 . (13) 

2=1 2=1 

H /r( „ has non-positive eigenvalues, therefore the maximum eigenvalue of the modified Davenport 
matrix is zero and the optimal solution is the identity quaternion. 

Re-introducing the error in the measurements and using y, and T (q) n, in the q-method the 
algorithm returns the estimation error since the performance index becomes 

J * (5q*) = trace [T (6c?) T(q)B T ] , (14) 

or equivalently (making use of the definition of the quaternion conjugate q* = [— qj <i\\ ) 


J* («Jq) 


o + (iq 1 


B e 

5z T 



(15) 


where 

n 

H e = 5>(|*x] [(Trij) x] + [(Tfij) x] [y*x]) (16) 

2=1 

n 

5z = -x: a i (y i X Trij) 

2=1 
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In the absence of noise the optimal eigenvalue is equal to zero. With noise, the optimal eigenvalue 
is a small quantity 5 A. The eigenvalue problem requires the following equation to be satisfied 

Hg <5q. (; + dq 4 Sz = 5 A (5q„. 

Making a first-order approximation of the quaternion and neglecting terms of order higher than one 
the estimation error is found to be 

5q y = — H^ 1 (5z. 

Therefore the covariance of the estimation error is given by 

P 0e = m e 1 E{6z5z T }(Hg 1 ) T , 

which is equivalent to the result by Shuster but derived differently. Clearly since the true attitude is 
unknown, Hg needs to be evaluated at the estimated attitude; the added approximation is a second- 
order effect. To first-order we have that 

n 

Sz = - y~] a t {y* x (T5nj) + Jy* x (Tri*)} . (18) 

i — 1 

therefore assuming each source of error is uncorrelated from the others 

n 

E {8zSz t } = Y j ( $ {[y*x] T E {SniSnJ} T t [y*x] T + [(Tn^)x] E {5 yi 5yf } [(T ni )x] T } . 

i=l 

(19) 

To calculate E {5z5z L } the unknown quantities y t , n j, and T need to be replaced with the known 
quantities y?, W, and T. 

Therefore, for reasons that will become evident in the subsequent section, suppose one defines R 
as 

R = 4E{5z<5z t } =4^o? [[y i x]TR nn T T [y i x] T + [(tfii)x] R yy [(fn 4 )x] T | (20) 

i=i { ' 

After substituting in Eq. (5b) and Eq. (5a), it follows that to first-order 

R = -2He. (21) 

This relationship will be critical in showing the equivalence between the qEKF and the SOAR filter. 

INITIAL CONDITION 

Shuster [19] shows one method to introduce initial conditions and quaternion measurements into 
the Wahba problem. Here a different approach is used. This section only treats the inclusion of 
the initial condition q 0 , which is equivalent to having a single quaternion measurement available 
on top of the vector measurements. The extension to multiple quaternion measurements is trivial; 
quaternion “measurements” are usually obtained from pre-processing vector measurements, in this 
work it is preferred to process the vector measurements directly. 
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where 

K e = (-Ao + Hfl)- 1 . 

This covariance update equation is now rewritten in the familial - Joseph form [21] 

(-A 0 + He )’ 1 (-A 0 ) = (- A 0 + He )" 1 (- A 0 + He - H 0 ) 
= I — (— A 0 + He ) -1 He 
= I - K e He 
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applying this result into Eq. (25b) 


= (I - K fl H 0 )<$q„o - KgSz. 

it then follows that 

Peg = (I - K 0 H 0 ) P eeo (I - K 0 H e ) T + K 0 RKq (28) 

which is the Joseph formula. The initial weight is chosen as Ao = 2P^j (| because the first term of 
Eq. (22) does not contain the factor 1/2 and 56 ~ 2E (q 0 ) 1 q. 

THE Q-METHOD EXTENDED KALMAN FILTER 

Let’s start with linear measurements, the extension to the nonlinear case can be readily obtained 
using standard extended Kalman filter techniques. Let y be a set of measurements of a state vector 
x corrupted by zero mean noise rj with covariance R 

y = Hx + rj, (29) 

where H is the measurement mapping (or sensitivity) matrix. Let x" be an unbiased estimate of x 
with corresponding estimation error covariance given by P~. The a priori estimation error is given 
by 

e = x — x . (30) 

The unbiased linear update based upon x~ and y produces the a posteriori estimate given by 

x + = x~ + K (y — Hx') = x~ + Ke, (31) 

where e is called the measurement residual and K is some deterministic matrix of appropriate 
dimensions to be determined. The a posteriori estimation error is expressed as 

e + = x — x + = (I - KH)e - Kr?. (32) 

Assuming that the measurement error rj and the a priori estimation error, e _ are uncorrelated and 
each are zero mean, we find that the a posteriori estimation error covariance is given by the Joseph 
formula [21] 

P+ = E | (x — x + ) (x — x + ) T | = (I-KH)P~(I-KH) T + KRK T , (33) 

where I is the identity matrix of appropriate dimension. Notice that no assumptions have been made 
as to the choice of K and the Joseph update equation is valid for all K. 

Suppose now that we partition x into 3 attitude states, 0, and n — 3 other states, s as 



(34) 

(35 a) 
(35b) 
(35c) 
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where W is the residuals covariance matrix 


W = HP'H t + R 

= H S P S - Hj + H S P~ H£ + HgP” H s r + HgP e0 H£ + R. (36) 

The updated portions of the covariance are 

r — “i r _ -i T 

P+=P SS -K S H - p! s H T Kj+K s WKj (37a) 

F 0s J L-^^s. 

r - -i r _ -| T 

P+ =P S0 -K S H pi* - pi s H T Kj+K s WKj (37b) 

F oo J Fes. 

r _ -i r - -i T 

P es = P e s ' K 0H - p* e HKj+KgWK^ (37c) 

L-^^sJ Feel 

r — i r - -i T 

P£g = Pgg-KgH £i* - ^i* H t k£ +K 0 WK£ (37d) 

. 99 J P 99. 

These equations are derived from the Joseph formula and are therefore valid for any choice K s and 
Kg. 

We now choose the K s and Kg carefully such that the gain for the states is the optimal value 
K S:0pt from Eq. (35c) and allowing Kg to be (as yet) unspecified. Thus, substituting for the value 
of K St opt for the three components in Eqs. (37a)-(37c), it becomes 

r r p i 

P — K ,WK t P _ — K ,H s * 

r ss ^s, opt vv -TVs, opt r s 0 -TVs.opr-n- -p- 

p+ _ lr 991 

tr — r_-iT r -| r n T 

p- _ P s0 H t K t P — KoH ^* s * — P s0 H t K t 4- K„WK t 

*0s p O *^-s,opt “ 99 p p 1 1 *^-9 ' " **-0 

l r 991 L r 99 J 1 r 99 J 

(38) 

This equation is valid for any value of Kg. Notice that there is no Kg in the cross-covariance 
between s and 0. Therefore, what is remarkable about this equation is that once the optimal K s op/ 
is chosen, the cross-covariance between s and 0 is independent of the choice of Kg. This property is 
fundamental to the development of the q-method EKF because it allows us to choose Kg as defined 
in the previous section 

Kg = (-A 0 + Hg)- 1 . 

The measurement residual is obtained as 

e = 2Kg 1 vec^q + ® (q - )*) 

where vec(q) is the function that returns the vector paid of the quaternion. 

It is assumed that the vector measurements are only functions of the vehicle attitude and do not 
depend on any other states, i.e. H s = 0. In summary the proposed algorithm has a propagation 
phase identical to that of the multiplicative EKF and an update phase as follows 


1 . Calculate the Davenport matrix K associated with all attitude vector measurements 



2. Calculate Ao = 2(P gg ) 1 

3. Calculate the updated attitude quaternion as the unit eigenvector associated with the maxi- 
mum eigenvalue of 



4. Calculate H 0 , R, Kg using Eqs. (16), (26), (19), and (28) 

5. Update the non attitude states as 

S = S T K s,opt € 

K s , opt = P s - 0 HT(H 0 P ee H£ + R )” 1 

e = 2Kg 1 vec^4 + <8> (<| - )*) 

6. Update the total covariance using Eq. (38) and H = [O H 0 ] . 

7. Process any remaining measurements using the standard MEKF algorithm 


COMPARISON WITH THE SOAR FILTER 

This section demonstrates the equivalence of the qEKF and the SOAR filter. It begins by making 
a key observation about the attitude profile matrix, and then proceeds to compare the attitude update 
and the non-attitude update. 


Observations on Computation of the Attitude Profile Matrix 

Begin by recalling that the Wahba Problem objective function given in Eq. (8) is the negative log- 
likelihood function when a* are chosen as shown in Eq. (6). The attitude may be expanded about 
the estimate using a Taylor Series expansion truncated to second-order 


J (86) = —trace 


1:3x3 + [— <$#x] + — [— 50x] 2 


TB J 


(39) 


Under mild conditions, the Fisher information matrix, Tee i s the expected value of the second- 
order derivative of the negative log-likelihood function. Recall from the Cramer-Rao inequality that 
the attitude covariance, P 00 , is related to the Fisher information matrix by [22] 


P ee < Te = E 


'd 2 j(6ey 

dsodse 


(40) 


and that Tee approaches P gg as the number of measurements become large. 

Because Eq. (40) requires the second derivative of J (36) with respect to 36 , terms in J (86) that 
are independent of 86 or linear in 86 are unimportant in the computation of Tee- Therefore, 


Te = E 


'd 2 j(8ey 

886 886 


= E 


r d 2 

886 886 


trace 


1 

2 


[- 60 x] 2 TB 



(41) 
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To compact notation, define the matrix V = TB 1 , 

^ = ^[M(- trace M 9 x l 2 v l)] <42) 

Now, making the observation that, 

[-sex] 2 = 50 S0 T - S6 t S 61 3x3 (43) 

one may directly rewrite Eq. (42) as 

Too = ~\e d8 d ed5e (trace [SO S0 T \] - S0 T S0 trace [V]) (44) 

Taking advantage of the cyclic properties of the trace operator, 

T W = ~\ E d5 ° ed5e {M T VS6 - trace [V] SQ T S0) (45) 

Straightforward differentiation will yield. 

Fee = trace [V] I 3x3 - \ (V + V T ) (46) 


In the presence of perfect measurements one may note that V = V 1 , and it is under these conditions 
that one arrives at the result presented by Shuster in [19]. In general, however, this is not the case 
and using Shuster’s formulation will result in nonsymmetric information and covariance matrices 
(clearly not correct!). Fortunately, this assumption is not necessary and one may correctly compute 
the Fisher information matrix in the presence of noise as, 

P^ 1 ~ Tee = trace [TB t ] I 3x3 - \ (TB t + BT t ) (47) 

The solution provided by Shuster in Ref. [19] to compute B from Fee and T is still valid. By 
taking the trace of Eq. (47), note that 

trace [Fee] = 3trace [TB 1 ] — trace [TB 1 ] = 2trace [TB 1 ] (48) 

Substituting this back into Eq. (47), 

TB t + BT 1 = trace [Fee] I 3 x 3 — 2Tee (49) 

Now, it is straightforward to verify that the following solution originally given by Shuster in [19] is 
also a solution to this equation, 

B = -trace [Fee] I 3 x 3 — Tee T (50) 
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Equivalence of the Attitude Update 

Recall from [17] that the SOAR filter includes the a-priori attitude information through the fol- 
lowing term in the objective function 

— q 1 K q = —trace T (B~) T (51) 

In [17] it is also shown that, after a second-order expansion of the matrix exponential of [— x] 
about the a priori attitude, this objective function may be rewritten as 

-4 T K-q = - (q-) T KT<r + ^86 T T ee S6 (52) 

The first term is a constant (not dependent on the a posteriori attitude) and disappears when the first 
differentials are taken to compute the optimal attitude. 

It is now straightforward to show that the a priori attitude term introduced in Eq. (22) is equivalent 
to 1/286 T FqqSQ to second-order. Thus, both the qEKF and the SOAR filters can be shown to 
include the a priori attitude information in an equivalent manner to second-order. 

To show this, begin by noting that 



Taking the Taylor Series expansion of sin (86/ 2), one may show that to second-order 



86 1 /<50\ 3 1 ( S0 \ 5 

~2 ~ 3 VT ) + 5 \2~ J 


86 

~2~ 


(54) 


Therefore, the first term in Eq. (22) may be rewritten as, 

q T S (qo) a oH (q 0 ) T q « ^d0 r A o <56> (55) 

Noting from before that Ao was chosen as A 0 — 2 (P 0e ) ~ 2 Tee, this directly yields 

q T 2 (<to) a oS (q 0 ) T q ~ ^ Sd^FeeSd (56) 

Therefore, the a priori attitude additions to the objective function for both SOAR and the qEKF are 
equivalent to second-order. 


Equivalence of the Non-Attitude Update 

Partition the Fisher information matrix of the full covariance as 


1 _ 

73 

Cfl 

Cfl 

F s e 

-l 

T7> 

’ F ss 

1 

Cfi 

fa 


73 

Cb 
c n 

Pee 

-txx- 

F^s 

Fgg \ 


The relation between Tqq and stems from the inversion of a partitioned matrix. 

Fee = P^ 1 + Fg s FjgF s e = Tee + Fg s FjgF s 0 


(57) 

(58) 
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And with a bit more simplification, 

K s , opt = -^P s - e 1,3x3 -He (—2 (P^) -1 + H e ) _1 = -^ Pkx3 - H e K e ] (63) 


Substituting into Eq. (60) for K Sj0pt , 


S + = S~ - P s -0 [I 3x3 - H 0 K e ] K 0 1 S (q ) T 

(64) 

Expanding terms, 

s+ = s- - p;„ [K ^ 1 - H e ] 3(r)V 

(65 a) 

= s "+ 2p ,“e( P M) _1 s (5“) 

(65b) 

Now, recall from the definition of the partitioned matrix inverse that 

p ,e ~ P ss p s e ( p ee) 

(66a) 

( p s S )~‘ p se — _p se ( p ee) 1 

(66b) 
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And, substituting this into Eq. (65b), 

s + = s- - 2 (F” ) -1 F“ 3 (V) q + (67a) 

s+^s--(F-)- 1 F-^ (67b) 

which arc equivalent to the SOAR Filter updates from Eq. (59). 

NUMERICAL EXAMPLE 

For this numerical example the spacecraft is placed in a circular orbit with an altitude of 622 km 
and an inclination of 45 degrees. At the beginning of the simulation the Earth is at vernal equinox 
20 March 2012 and the spacecraft is at the ascending node. Throughout its orbit the spacecraft is 
oriented such that the body-fixed X axis is directed in track and the Z axis is Earth-pointing with the 
Y axis following a right handed coordinate system. As a result the spacecraft has a constant angular 
velocity equal in magnitude to the orbital mean motion. The sun vector is assumed constant for the 
duration of the simulation. The magnetic field vector is obtained from the World Magnetic Model 
in the MATLAB Aerospace toolbox. 

A gyro is used to measure the angular velocity of the spacecraft and is defined by the following 
sensor model [23] 

u = u + (3 + T] v 

P = Vu 

where u) is the true angular velocity, u> is the measured angular velocity, (3 is the gyro bias vector, 
and r] v and r/ u are zero-mean Gaussian white-noise processes. Simulated vectors measurements are 
created by adding noise to the true direction in the spacecraft body frame. The reference vectors 
remain noise free as the model is assumed perfect for this test case. The scalar weights a* of the 
Wahba problem follow the QUEST measurement model and arc given by 1 / cr 2 sun and 1 / af nag for 
the sun sensor and magnetometer measurements respectively. 


Error Source 

Symbol 

Value 

Sun-sensor noise (■ rj sun ) 

G sun 

0. 1 deg 

Magnetometer noise (?7 ma9 ) 

&mag 

0.5 deg 

Angular Random Walk (rj v ) 

G v 

x/10 x 10“' rad/sec 1 / 2 

Gyro Bias Random Walk (rj u ) 


x/IO x 10“ 10 rad/sec 3 / 2 


Table 1. Sensors Errors 


The state vector consists of the three component gyro bias vector and the three component attitude 
angle representation x 1 = [/3 T d T ] . The initial gyro bias covariance is 0.2 2 (deg/hr) 2 in each 
axis and the initial attitude covariance is 0.1 2 deg 2 in each axis. The initial estimated quaternion is 
obtained from perturbing the true quaternion according to the initial attitude covariance while the 
initial estimated gyro bias is always zero. The simulation spans 6000 seconds which is slightly more 
than one full orbit and uses a step size between observations of 1 second. 

Figs. 1 and 2 show the performance of 100 Monte Carlo runs. The red lines are the 100 instances 
of the estimation error, the black line is the 3-sigma value of the sample standard deviation. Under- 
neath the black lines there are 100 blue lines, which arc the 3-sigma filter’s prediction of its own 
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uncertainty. Since the predicted uncertainty matches the actual uncertainty we conclude that the 
filter is consistent. Fig. 3 shows the performance of SOAR under the same circumstances. It can be 
seen that there is no visible difference between the two algorithms. 




Time (sec) 


Figure 1. Attitude estimation error of qEKF expressed in body frame 


CONCLUSIONS 

The q-method was integrated into an EKF-based filter to produce the novel qEKF filter for attitude 
estimation capable of estimating both attitude and non-attitude states without additional numerical 
iteration. Within the filter attitude vector measurements are first processed using the q-method which 
solves the non-linear Wahba problem directly without any linearizing assumptions. Remaining 
measurements are processed to update the non-attitude states using the standard MEKF algorithm. 
qEKF was shown to be equivalent to the Sequential Optimal Attitude Recursion (SOAR) filter to 
second-order in the attitude update and to first-order in the non-attitude state update where each 
method represents the covariance and information matrix formulation respectively. In qEKF the 
initial condition is introduced into the Wahba problem through quaternion averaging where the 
SOAR filter relies on the information matrix approach. The equivalence of qEKF and SOAR was 
also validated by simulation results in which the filter estimated the attitude and gyro bias. 

For this work it was assumed that vector measurements are only functions of the vehicle attitude 
and not dependent on any other states. A follow-on paper will expand upon this work to extend 
qEKF to eliminate this assumption and accommodate the inclusion of such states as sensor biases 
and satellite position. As a result, qEKF will be capable of processing a full range of measurements 
to estimate both attitude and non-attitude states within a single filter. 
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Figure 2. Gyro bias estimation error of qEKF 
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Figure 3. Attitude estimation error of SOAR expressed in body frame 
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